
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_copter.c
  * @author     baiyang
  * @date       2022-11-17
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <rtthread.h>

#include <rc_channel/rc_channel.h>
/*-----------------------------------macro------------------------------------*/
#define NUM_FLIGHT_MODES 6
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void init_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos ch_flag);
static bool do_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos ch_flag);
static void mode_switch_changed(rc_channel_t rc, modeswitch_pos_t new_pos);
static void rc_do_aux_function_change_mode(const ModeNumber mode, const enum RcAuxSwitchPos ch_flag);

/// rcs
static bool has_valid_input(rcs_channel_t rcs);
static rc_channel_t get_arming_channel(rcs_channel_t rcs);
static rc_channel_t channel(rcs_channel_t rcs, const uint8_t chan);
static bool arming_check_throttle(rcs_channel_t rcs);
static int8_t flight_mode_channel_number(rcs_channel_t rcs);
/*----------------------------------variable----------------------------------*/
struct rc_channel obj_channels[NUM_RC_CHANNELS];

static struct rc_ops rc_channel_ops = {.init_aux_function   = init_aux_function,
                                       .do_aux_function     = do_aux_function,
                                       .mode_switch_changed = mode_switch_changed};

static struct rcs_ops rc_channels_ops = {.channel = channel,
                                         .read_mode_switch = NULL,
                                         .has_valid_input = has_valid_input,
                                         .get_arming_channel = get_arming_channel,
                                         .arming_check_throttle = arming_check_throttle,
                                         .flight_mode_channel_number = flight_mode_channel_number};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void init_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos ch_flag)
{
    // init channel options
    switch(ch_option) {
    // the following functions do not need to be initialised:
    case RC_AUX_ALTHOLD:
    case RC_AUX_AUTO:
    case RC_AUX_AUTOTUNE:
    case RC_AUX_BRAKE:
    case RC_AUX_CIRCLE:
    case RC_AUX_DRIFT:
    case RC_AUX_FLIP:
    case RC_AUX_FLOWHOLD:
    case RC_AUX_FOLLOW:
    case RC_AUX_GUIDED:
    case RC_AUX_LAND:
    case RC_AUX_LOITER:
    case RC_AUX_PARACHUTE_RELEASE:
    case RC_AUX_POSHOLD:
    case RC_AUX_RESETTOARMEDYAW:
    case RC_AUX_RTL:
    case RC_AUX_SAVE_TRIM:
    case RC_AUX_SAVE_WP:
    case RC_AUX_SMART_RTL:
    case RC_AUX_STABILIZE:
    case RC_AUX_THROW:
    case RC_AUX_USER_FUNC1:
    case RC_AUX_USER_FUNC2:
    case RC_AUX_USER_FUNC3:
    case RC_AUX_WINCH_CONTROL:
    case RC_AUX_ZIGZAG:
    case RC_AUX_ZIGZAG_Auto:
    case RC_AUX_ZIGZAG_SaveWP:
    case RC_AUX_ACRO:
    case RC_AUX_AUTO_RTL:
    case RC_AUX_TURTLE:
    case RC_AUX_SIMPLE_HEADING_RESET:
    case RC_AUX_ARMDISARM_AIRMODE:
    case RC_AUX_TURBINE_START:
        break;
    case RC_AUX_ACRO_TRAINER:
    case RC_AUX_ATTCON_ACCEL_LIM:
    case RC_AUX_ATTCON_FEEDFWD:
    case RC_AUX_INVERTED:
    case RC_AUX_MOTOR_INTERLOCK:
    case RC_AUX_PARACHUTE_3POS:      // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
    case RC_AUX_PARACHUTE_ENABLE:
    case RC_AUX_PRECISION_LOITER:
    case RC_AUX_RANGEFINDER:
    case RC_AUX_SIMPLE_MODE:
    case RC_AUX_STANDBY:
    case RC_AUX_SUPERSIMPLE_MODE:
    case RC_AUX_SURFACE_TRACKING:
    case RC_AUX_WINCH_ENABLE:
    case RC_AUX_AIRMODE:
    case RC_AUX_FORCEFLYING:
    case RC_AUX_CUSTOM_CONTROLLER:
        rc_run_aux_function(rc, ch_option, ch_flag, RC_FUNC_TRIGGER_SOURCE_INIT);
        break;
    default:
        rc_InitAuxFunction(rc, ch_option, ch_flag);
        break;
    }
}

static bool do_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos ch_flag)
{
    switch(ch_option) {

        case RC_AUX_RTL:
            rc_do_aux_function_change_mode(RTL, ch_flag);
            break;

        case RC_AUX_SAVE_WP:
            // save waypoint when switch is brought high
            if (ch_flag == RC_AUX_SWITCH_HIGH) {

                // do not allow saving new waypoints while we're in auto or disarmed
                if (fms.flightmode == (mode_base_t)&fms.mode_auto || !fms.motors->_armed) {
                    break;
                }

                // do not allow saving the first waypoint with zero throttle
                if ((mission_num_commands() == 0) && (rc_get_control_in(fms.channel_throttle) == 0)) {
                    break;
                }

                // create new mission command
                struct Mission_Command cmd  = {0};

                // if the mission is empty save a takeoff command
                if (mission_num_commands() == 0) {
                    // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
                    cmd.id = MAV_CMD_NAV_TAKEOFF;
                    cmd.content.location.alt = MAX(fms.current_loc.alt,100);

                    // use the current altitude for the target alt for takeoff.
                    // only altitude will matter to the AP mission script for takeoff.
                    if (mission_add_cmd(&cmd)) {
                        // log event
                        //AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP);
                    }
                }

                // set new waypoint to current location
                cmd.content.location = fms.current_loc;

                // if throttle is above zero, create waypoint command
                if (rc_get_control_in(fms.channel_throttle) > 0) {
                    cmd.id = MAV_CMD_NAV_WAYPOINT;
                } else {
                    // with zero throttle, create LAND command
                    cmd.id = MAV_CMD_NAV_LAND;
                }

                // save command
                if (mission_add_cmd(&cmd)) {
                    // log event
                    //AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP);
                }
            }
            break;
        case RC_AUX_AUTO:
            rc_do_aux_function_change_mode(AUTO, ch_flag);
            break;

        case RC_AUX_AUTOTUNE:
            rc_do_aux_function_change_mode(AUTOTUNE, ch_flag);
            break;

        case RC_AUX_LAND:
            rc_do_aux_function_change_mode(LAND, ch_flag);
            break;

        case RC_AUX_GUIDED:
            rc_do_aux_function_change_mode(GUIDED, ch_flag);
            break;

        case RC_AUX_LOITER:
            rc_do_aux_function_change_mode(LOITER, ch_flag);
            break;

        case RC_AUX_FOLLOW:
            rc_do_aux_function_change_mode(FOLLOW, ch_flag);
            break;


        case RC_AUX_MOTOR_INTERLOCK:
#if 0
#if FRAME_CONFIG == HELI_FRAME
            // The interlock logic for ROTOR_CONTROL_MODE_PASSTHROUGH is handled 
            // in heli_update_rotor_speed_targets.  Otherwise turn on when above low.
            if (copter.motors->get_rsc_mode() != ROTOR_CONTROL_MODE_PASSTHROUGH) {
                copter.ap.motor_interlock_switch = (ch_flag == AuxSwitchPos::HIGH || ch_flag == AuxSwitchPos::MIDDLE);
            }
#else
            copter.ap.motor_interlock_switch = (ch_flag == AuxSwitchPos::HIGH || ch_flag == AuxSwitchPos::MIDDLE);
#endif
#endif
            fms.ap.motor_interlock_switch = (ch_flag == RC_AUX_SWITCH_HIGH || ch_flag == RC_AUX_SWITCH_MIDDLE);
            break;

        case RC_AUX_BRAKE:
#if MODE_BRAKE_ENABLED == ENABLED
            rc_do_aux_function_change_mode(BRAKE, ch_flag);
#endif
            break;

        case RC_AUX_THROW:
#if MODE_THROW_ENABLED == ENABLED
            rc_do_aux_function_change_mode(THROW, ch_flag);
#endif
            break;

        case RC_AUX_SMART_RTL:
#if MODE_SMARTRTL_ENABLED == ENABLED
            rc_do_aux_function_change_mode(SMART_RTL, ch_flag);
#endif
            break;

        case RC_AUX_WINCH_CONTROL:
            // do nothing, used to control the rate of the winch and is processed within AP_Winch
            break;

        case RC_AUX_ZIGZAG:
#if MODE_ZIGZAG_ENABLED == ENABLED
            rc_do_aux_function_change_mode(ZIGZAG, ch_flag);
#endif
            break;

        case RC_AUX_STABILIZE:
            rc_do_aux_function_change_mode(STABILIZE, ch_flag);
            break;

        case RC_AUX_POSHOLD:
#if MODE_POSHOLD_ENABLED == ENABLED
            rc_do_aux_function_change_mode(POSHOLD, ch_flag);
#endif
            break;

        case RC_AUX_ALTHOLD:
            rc_do_aux_function_change_mode(ALT_HOLD, ch_flag);
            break;


        case RC_AUX_ACRO:
#if MODE_ACRO_ENABLED == ENABLED
            rc_do_aux_function_change_mode(ACRO, ch_flag);
#endif
            break;

        case RC_AUX_FLOWHOLD:
            rc_do_aux_function_change_mode(FLOWHOLD, ch_flag);
            break;

        case RC_AUX_CIRCLE:
            rc_do_aux_function_change_mode(CIRCLE, ch_flag);
            break;

        case RC_AUX_DRIFT:
#if MODE_DRIFT_ENABLED == ENABLED
            rc_do_aux_function_change_mode(DRIFT, ch_flag);
#endif
            break;

        case RC_AUX_STANDBY: {
            switch (ch_flag) {
                case RC_AUX_SWITCH_HIGH:
                    fms.standby_active = true;
                    //AP::logger().Write_Event(LogEvent::STANDBY_ENABLE);
                    gcs_send_text(MAV_SEVERITY_INFO, "Stand By Enabled");
                    break;
                default:
                    fms.standby_active = false;
                    //AP::logger().Write_Event(LogEvent::STANDBY_DISABLE);
                    gcs_send_text(MAV_SEVERITY_INFO, "Stand By Disabled");
                    break;
                }
            break;
        }

        case RC_AUX_AUTO_RTL:
#if MODE_AUTO_ENABLED == ENABLED
            rc_do_aux_function_change_mode(AUTO_RTL, ch_flag);
#endif
            break;

        case RC_AUX_TURTLE:
#if MODE_TURTLE_ENABLED == ENABLED
            rc_do_aux_function_change_mode(TURTLE, ch_flag);
#endif
            break;

    default:
        return rc_DoAuxFunction(rc, ch_option, ch_flag);
    }

    return true;
}

// called when the mode switch changes position:
static void mode_switch_changed(rc_channel_t rc, modeswitch_pos_t new_pos)
{
    if (new_pos < 0 || (uint8_t)new_pos > NUM_FLIGHT_MODES) {
        // should not have been called
        return;
    }

    if (!fms_set_mode((ModeNumber)fms.flight_modes[new_pos], MODE_REASON_RC_COMMAND)) {
        return;
    }

    if (!rcs_find_channel_for_option(RC_AUX_SIMPLE_MODE) &&
        !rcs_find_channel_for_option(RC_AUX_SUPERSIMPLE_MODE)) {
        // if none of the Aux Switches are set to Simple or Super Simple Mode then
        // set Simple Mode using stored parameters from EEPROM
#if 0
        if (BIT_IS_SET(copter.g.super_simple, new_pos)) {
            copter.set_simple_mode(Copter::SimpleMode::SUPERSIMPLE);
        } else {
            copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos) ? Copter::SimpleMode::SIMPLE : Copter::SimpleMode::NONE);
        }
#endif
    }
}

// do_aux_function_change_mode - change mode based on an aux switch
// being moved
static void rc_do_aux_function_change_mode(const ModeNumber mode, const enum RcAuxSwitchPos ch_flag)
{
    switch(ch_flag) {
    case RC_AUX_SWITCH_HIGH: {
        // engage mode (if not possible we remain in current flight mode)
        fms_set_mode(mode, MODE_REASON_RC_COMMAND);
        break;
    }
    default:
        // return to flight mode switch's flight mode if we are currently
        // in this mode
        if (mode_number(fms.flightmode) == mode) {
            rcs_reset_mode_switch();
        }
    }
}

/// rc_channels
static bool has_valid_input(rcs_channel_t rcs)
{
    (void)rcs;

    if (fms.failsafe.radio) {
        return false;
    }
    if (fms.failsafe.radio_counter != 0) {
        return false;
    }
    return true;
}

static rc_channel_t get_arming_channel(rcs_channel_t rcs)
{
    //return fms.channel_yaw;
    return NULL;
}

static rc_channel_t channel(rcs_channel_t rcs, const uint8_t chan)
{
    if (chan >= NUM_RC_CHANNELS) {
        return NULL;
    }
    return &obj_channels[chan];
}

// returns true if throttle arming checks should be run
static bool arming_check_throttle(rcs_channel_t rcs)
{
    if ((fms.g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
        // center sprung throttle configured, dont run AP_Arming check
        // Copter already checks this case in its own arming checks
        return false;
    }
    return rcs_ArmingCheckThrottle();
}

static int8_t flight_mode_channel_number(rcs_channel_t rcs)
{
    return fms.flight_mode_chan;
}

// 
static int rcs_copter_ctor(void)
{
    rcs_ctor(&rc_channels_ops);

    // setup ch_in on channels
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        rc_ctor(&obj_channels[i], &rc_channel_ops);
    }

    return 0;
}
INIT_COMPONENT_EXPORT(rcs_copter_ctor);
/*------------------------------------test------------------------------------*/


